
/** \file main.cpp
    \author Peter Anastasiou and Laurence McLean
    
    Runs a simple robot arm
*/

#include "src/robotArm.hpp"

int main(int argc, char ** argv) {
  
  bool test = false;
  
  if(!test) {
    // Business as usual:
    cppexamples::RobotArm example;
    return example.main(argc, argv);
    
  } else {
    // --------------- Run Tests ---------------
    
    // Test IK & FK:
    RobotModel* robot = new RobotModel();
    robot->setLengths(0.0, 0.200, 0.320, 0.550, 0.500);
    JointSpaceSolver* solver = new JointSpaceSolver(robot);
    
    cvec4d* cpos = new cvec4d(0.5, 0.4, -0.1, 2*M_PI/3.0);
    printf("Cartesian Position: ");
    cpos->println();
    
    jvec4d* jpos = new jvec4d();
    solver->solveIK(cpos, jpos);
    printf("IK Joint Positions: ");
    jpos->println();
    
    solver->solveFK(jpos, cpos);
    printf("FK Cart Position: ");
    cpos->println();
    
    delete robot;
    delete solver;
    delete jpos;
    delete cpos;
    
    // Test Joint Trajectory Interpolation:
//    Trajectory* traj = new Trajectory();
//    jvec4d* jpos = new jvec4d(0, 0, 0, 10);
//    traj->addSetpoint(1, jpos);
//    jpos->set(8, 0, 1, 0);
//    traj->addSetpoint(100, jpos);
//    jpos->set(0.0, 5.0, 0, -10);
//    traj->addSetpoint(200, jpos);
//    
//    jvec4d* jointPositions = new jvec4d();
//    
//    for(uint32_t i = 0; i < 20; i ++) {
//      traj->getSetpoint(i, jointPositions);
//      std::cout << i << " ";
//      jointPositions->print_rad();
//      std::cout << "\n";
//    }
//    for(uint32_t i = 20; i < 300; i += 10) {
//      traj->getSetpoint(i, jointPositions);
//      std::cout << i << " ";
//      jointPositions->print_rad();
//      std::cout << "\n";
//    }
//    
//    delete jointPositions;
//    delete traj;
    return 0;
  }
}







